﻿/*
* Copyright (c) 2010-2012 Tesla Engine Group
* 
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
* 
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
* 
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/


using System;
using System.Globalization;
using System.Runtime.InteropServices;
using Tesla.Util;

namespace Tesla.Math {

	/// <summary>
	/// Defines a 4 dimensional vector that represents a rotation, where
	/// the x,y,z components are a vector that an object is rotated about 
	/// by the angle theta. W is equal to cos(theta/2).
	/// </summary>
	[Serializable]
	[StructLayout(LayoutKind.Sequential)]
	public struct Quaternion : IEquatable<Quaternion> {

		/// <summary>
		/// X component of the vector part of the quaternion.
		/// </summary>
		public float X;

		/// <summary>
		/// Y component of the vector part of the quaternion.
		/// </summary>
		public float Y;

		/// <summary>
		/// Z component of the vector part of the quaternion.
		/// </summary>
		public float Z;

		/// <summary>
		/// Rotation component of the quaternion.
		/// </summary>
		public float W;

		private static Quaternion _identity = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);

        private static int _sizeInbytes = MemoryHelper.SizeOf<Quaternion>();

		/// <summary>
		/// Gets the Quaternion identity - (0, 0, 0, 1).
		/// </summary>
		public static Quaternion Identity {
			get {
				return _identity;
			}
		}

		/// <summary>
		/// Gets the size of a Quaternion in bytes.
		/// </summary>
		public static int SizeInBytes {
			get {
				return _sizeInbytes;
			}
		}

		/// <summary>
		/// Creates a new instance of Quaternion.
		/// </summary>
		/// <param name="value">Vector part</param>
		/// <param name="w">Rotation component</param>
		public Quaternion(Vector3 value, float w) {
			X = value.X;
			Y = value.Y;
			Z = value.Z;
			W = w;
		}

		/// <summary>
		/// Creates a new instance of Quaternion.
		/// </summary>
		/// <param name="x">X component of the vector part</param>
		/// <param name="y">Y component of the vector part</param>
		/// <param name="z">Z component of the vector part</param>
		/// <param name="w">Rotation component</param>
		public Quaternion(float x, float y, float z, float w) {
			X = x;
			Y = y;
			Z = z;
			W = w;
		}

		/// <summary>
		/// Adds two quaternions together.
		/// </summary>
		/// <param name="a">First quaternion</param>
		/// <param name="b">Second quaternion</param>
		/// <returns></returns>
		public static Quaternion Add(Quaternion a, Quaternion b) {
			Quaternion q;
			q.X = a.X + b.X;
			q.Y = a.Y + b.Y;
			q.Z = a.Z + b.Z;
			q.W = a.W + b.W;
			return q;
		}

		/// <summary>
		/// Adds two quaternions together.
		/// </summary>
		/// <param name="a">First quaternion</param>
		/// <param name="b">Second quaternion</param>
		/// <param name="result">Existing quaternion that is the resulting quaternion</param>
		public static void Add(ref Quaternion a, ref Quaternion b, out Quaternion result) {
			result.X = a.X + b.X;
			result.Y = a.Y + b.Y;
			result.Z = a.Z + b.Z;
			result.W = a.W + b.W;
		}

		/// <summary>
		/// Flips the signs of the components of this quaternion.
		/// </summary>
		public void Negate() {
			this.X = -this.X;
			this.Y = -this.Y;
			this.Z = -this.Z;
			this.W = -this.W;
		}

		/// <summary>
		/// Flips the signs of the components of the specified quaternion.
		/// </summary>
		/// <param name="value">Source quaternion</param>
		/// <returns>Resulting quaternion</returns>
		public static Quaternion Negate(Quaternion value) {
			Quaternion q;
			q.X = -value.X;
			q.Y = -value.Y;
			q.Z = -value.Z;
			q.W = -value.W;
			return q;
		}

		/// <summary>
		/// Flips the signs of the components of the specified quaternion.
		/// </summary>
		/// <param name="value">Source quaternion</param>
		/// <param name="result">Existing quaternion that holds result</param>
		public static void Negate(ref Quaternion value, out Quaternion result) {
			result.X = -value.X;
			result.Y = -value.Y;
			result.Z = -value.Z;
			result.W = -value.W;
		}

		/// <summary>
		/// Subtracts two quaternions.
		/// </summary>
		/// <param name="a">First quaternion</param>
		/// <param name="b">Second quaternion</param>
		/// <returns>Resulting quaternion</returns>
		public static Quaternion Subtract(Quaternion a, Quaternion b) {
			Quaternion q;
			q.X = a.X - b.X;
			q.Y = a.Y - b.Y;
			q.Z = a.Z - b.Z;
			q.W = a.W - b.W;
			return q;
		}

		/// <summary>
		/// Subtracts two quaternions.
		/// </summary>
		/// <param name="a">First quaternion</param>
		/// <param name="b">Second quaternion</param>
		/// <param name="result">Existing quaternion that is the result</param>
		public static void Subtract(ref Quaternion a, ref Quaternion b, out Quaternion result) {
			result.X = a.X - b.X;
			result.Y = a.Y - b.Y;
			result.Z = a.Z - b.Z;
			result.W = a.W - b.W;
		}

		/// <summary>
		/// Multiplies two quaternions together.
		/// </summary>
		/// <param name="a">First quaternion</param>
		/// <param name="b">Second quaternion</param>
		/// <returns>Resulting quaternion</returns>
		public static Quaternion Multiply(Quaternion a, Quaternion b) {
			float x1 = a.X;
			float y1 = a.Y;
			float z1 = a.Z;
			float w1 = a.W;

			float x2 = b.X;
			float y2 = b.Y;
			float z2 = b.Z;
			float w2 = b.W;

			float num1 = (y1 * z2) - (z1 * y2);
			float num2 = (z1 * x2) - (x1 * z2);
			float num3 = (x1 * y2) - (y1 * x2);
			float num4 = ((x1 * x2) + (y1 * y2)) + (z1 * z2);

			Quaternion q;
			q.X = ((x1 * w2) + (x2 * w1)) + num1;
			q.Y = ((y1 * w2) + (y2 * w1)) + num2;
			q.Z = ((z1 * w2) + (z2 * w1)) + num3;
			q.W = (w1 * w2) - num4;

			return q;
		}

		/// <summary>
		/// Multiplies a quaternion by a scaling value.
		/// </summary>
		/// <param name="value">Source quaternion</param>
		/// <param name="scale">Scaling value</param>
		/// <returns>Resulting quaternion</returns>
		public static Quaternion Multiply(Quaternion value, float scale) {
			Quaternion q;
			q.X = value.X * scale;
			q.Y = value.Y * scale;
			q.Z = value.Z * scale;
			q.W = value.W * scale;
			return q;
		}

		/// <summary>
		/// Multiplies two quaternions.
		/// </summary>
		/// <param name="a">First quaternion</param>
		/// <param name="b">Second quaternion</param>
		/// <param name="result">Existing quaternion to hold result</param>
		public static void Multiply(ref Quaternion a, ref Quaternion b, out Quaternion result) {
			float x1 = a.X;
			float y1 = a.Y;
			float z1 = a.Z;
			float w1 = a.W;

			float x2 = b.X;
			float y2 = b.Y;
			float z2 = b.Z;
			float w2 = b.W;

			float num1 = (y1 * z2) - (z1 * y2);
			float num2 = (z1 * x2) - (x1 * z2);
			float num3 = (x1 * y2) - (y1 * x2);
			float num4 = ((x1 * x2) + (y1 * y2)) + (z1 * z2);

			result.X = ((x1 * w2) + (x2 * w1)) + num1;
			result.Y = ((y1 * w2) + (y2 * w1)) + num2;
			result.Z = ((z1 * w2) + (z2 * w1)) + num3;
			result.W = (w1 * w2) - num4;
		}

		/// <summary>
		/// Multiplies a quaternion by a scaling value.
		/// </summary>
		/// <param name="value">Source quaternion</param>
		/// <param name="scale">Scaling value</param>
		/// <param name="result">Existing quaternion to hold result</param>
		public static void Multiply(ref Quaternion value, float scale, out Quaternion result) {
			result.X = value.X * scale;
			result.Y = value.Y * scale;
			result.Z = value.Z * scale;
			result.W = value.W * scale;
		}

		/// <summary>
		/// Divides a quaternion by another.
		/// </summary>
		/// <param name="a">First quaternion</param>
		/// <param name="b">Second quaternion (divisor)</param>
		/// <returns>Resulting quaternion</returns>
		public static Quaternion Divide(Quaternion a, Quaternion b) {
			float x1 = a.X;
			float y1 = a.Y;
			float z1 = a.Z;
			float w1 = a.W;

			float x2 = b.X;
			float y2 = b.Y;
			float z2 = b.Z;
			float w2 = b.W;

			float num = (((x2 * x2) + (y2 * y2) + (z2 * z2) + (w2 * w2)));
			float invNum  = 1.0f / num;

			float num1 = -x2 * invNum;
			float num2 = -y2 * invNum;
			float num3 = -z2 * invNum;
			float num4 = -w2 * invNum;

			float num5 = (y1 * num3) - (z1 * num2);
			float num6 = (z1 * num1) - (x1 * num3);
			float num7 = (x1 * num2) - (y1 * num1);
			float num8 = ((x1 * num1) + (y1 * num2) + (z1 * num3));

			Quaternion q;
			q.X = ((x1 * num4) + (num1 * w1)) + num5;
			q.Y = ((y1 * num4) + (num2 * w1)) + num6;
			q.Z = ((z1 * num4) + (num3 * w1)) + num7;
			q.W = (w1 * num4) - num8;
			return q;
		}

		/// <summary>
		/// Divides a quaternion by another.
		/// </summary>
		/// <param name="a">First quaternion</param>
		/// <param name="b">Second quaternion (divisor)</param>
		/// <param name="result">Resulting quaternion</param>
		public static void Divide(ref Quaternion a, ref Quaternion b, out Quaternion result) {
			float x1 = a.X;
			float y1 = a.Y;
			float z1 = a.Z;
			float w1 = a.W;

			float x2 = b.X;
			float y2 = b.Y;
			float z2 = b.Z;
			float w2 = b.W;

			float num = (((x2 * x2) + (y2 * y2) + (z2 * z2) + (w2 * w2)));
			float invNum  = 1.0f / num;

			float num1 = -x2 * invNum;
			float num2 = -y2 * invNum;
			float num3 = -z2 * invNum;
			float num4 = -w2 * invNum;

			float num5 = (y1 * num3) - (z1 * num2);
			float num6 = (z1 * num1) - (x1 * num3);
			float num7 = (x1 * num2) - (y1 * num1);
			float num8 = ((x1 * num1) + (y1 * num2) + (z1 * num3));

			result.X = ((x1 * num4) + (num1 * w1)) + num5;
			result.Y = ((y1 * num4) + (num2 * w1)) + num6;
			result.Z = ((z1 * num4) + (num3 * w1)) + num7;
			result.W = (w1 * num4) - num8;
		}

		/// <summary>
		/// Transforms this quaternion into its conjugate.
		/// </summary>
		public void Conjugate() {
			this.X = -this.X;
			this.Y = -this.Y;
			this.Z = -this.Z;
		}

		/// <summary>
		/// Returns the conjugate of the specified quaternion.
		/// </summary>
		/// <param name="value">Source quaternion</param>
		/// <returns>Conjugate</returns>
		public static Quaternion Conjugate(Quaternion value) {
			Quaternion q;
			q.X = -value.X;
			q.Y = -value.Y;
			q.Z = -value.Z;
			q.W = value.W;
			return q;
		}

		/// <summary>
		/// Returns the conjugate of the specified quaternion.
		/// </summary>
		/// <param name="value">Source quaternion</param>
		/// <param name="result">Existing quaternion that is the conjugate of the source quaternion</param>
		public static void Conjugate(ref Quaternion value, out Quaternion result) {
			result.X = -value.X;
			result.Y = -value.Y;
			result.Z = -value.Z;
			result.W = -value.Z;
		}

		/// <summary>
		/// Computes the dot product of two quaternions.
		/// </summary>
		/// <param name="a">First quaternion</param>
		/// <param name="b">Second quaternion</param>
		/// <returns>Dot product</returns>
		public static float Dot(Quaternion a, Quaternion b) {
			return (a.X * b.X) + (a.Y * b.Y) + (a.Z * b.Z) + (a.W * b.W);
		}

		/// <summary>
		/// Computes the dot product of two quaternions.
		/// </summary>
		/// <param name="a">First quaternion</param>
		/// <param name="b">Second quaternion</param>
		/// <param name="result">Existing float to hold dot product</param>
		public static void Dot(ref Quaternion a, ref Quaternion b, out float result) {
			result = (a.X * b.X) + (a.Y * b.Y) + (a.Z * b.Z) + (a.W * b.W);
		}

		/// <summary>
		/// Compute the count (magnitude) of this quaternion.
		/// </summary>
		/// <returns>Length</returns>
		public float Length() {
			double lengthSquared = (double)((this.X * this.X) + (this.Y * this.Y) + (this.Z * this.Z) + (this.W * this.W));
			return (float)System.Math.Sqrt(lengthSquared);
		}

		/// <summary>
		/// Compute the count (magnitude) squared, of this quaternion.
		/// </summary>
		/// <returns>Length squared</returns>
		public float LengthSquared() {
			return (this.X * this.X) + (this.Y * this.Y) + (this.Z * this.Z) + (this.W * this.W);
		}

		/// <summary>
		/// Invert this quaternion.
		/// </summary>
		public void Invert() {
			float lengthSquared = (this.X * this.X) + (this.Y * this.Y) + (this.Z * this.Z) + (this.W * this.W);
			float invSqrt = 1.0f / lengthSquared;
			this.X = -this.X * invSqrt;
			this.Y = -this.Y * invSqrt;
			this.Z = -this.Z * invSqrt;
			this.W = -this.W * invSqrt;
		}

		/// <summary>
		/// Gets the row of a 3x3 rotation matrix as a vector from this quaternion (normalized).
		/// </summary>
		/// <param name="rot">Quaternion to get the column vector from</param>
		/// <param name="i">Index of column to compute, must be between 0-2</param>
		/// <returns>Column vector from the 3x3 rotation matrix</returns>
		public static Vector3 GetRotationVector(Quaternion rot, int i) {
			float norm = (rot.W * rot.W) + (rot.X * rot.X) + (rot.Y * rot.Y) + (rot.Z * rot.Z);

			float s = 0;

			//We must ensure the quaternion is normalized first. We also use 2 / norm to save mults later
			if(norm == 1.0f) {
				s = 2;
			} else if(norm > 0.0f) {
				s = 2 / norm;
			}

			//Compute xs/ys/zs since we use them 2-4 times, saves 6 mults
			float xs = rot.X * s;
			float ys = rot.Y * s;
			float zs = rot.Z * s;

			float xx = rot.X * xs;
			float xy = rot.X * ys;
			float xz = rot.X * zs;
			float xw = rot.W * xs;

			float yy = rot.Y * ys;
			float yz = rot.Y * zs;
			float yw = rot.W * ys;

			float zz = rot.Z * zs;
			float zw = rot.W * zs;

			Vector3 store;

			switch(i) {
				case 0:
					store.X  = 1 - (yy + zz);
					store.Y  =     (xy + zw);
					store.Z  =     (xz - yw);
					break;
				case 1:
					store.X  =     (xy - zw);
					store.Y  = 1 - (xx + zz);
					store.Z  =     (yz + xw);
					break;
				case 2:
					store.X  =     (xz + yw);
					store.Y  =     (yz - xw);
					store.Z  = 1 - (xx + yy);
					break;
				default:
					store = Vector3.Zero;
					break;
			}

			return store;
		}

		/// <summary>
		/// Gets the row of a 3x3 rotation matrix as a vector from this quaternion (normalized).
		/// </summary>
		/// <param name="rot">Quaternion to get the column vector from</param>
		/// <param name="i">Index of column to compute, must be between 0-2</param>
		/// <param name="store">Existing vector to hold result</param>
		/// <returns>Column vector from the 3x3 rotation matrix</returns>
		public static void GetRotationVector(ref Quaternion rot, int i, out Vector3 store) {
			float norm = (rot.W * rot.W) + (rot.X * rot.X) + (rot.Y * rot.Y) + (rot.Z * rot.Z);

			if(norm != 1.0f) {
				norm = 1.0f / (float) System.Math.Sqrt(norm);
			}

			float xnorm = rot.X * norm;
			float ynorm = rot.Y * norm;
			float znorm = rot.Z * norm;
			float wnorm = rot.W * norm;

			float xx = rot.X * xnorm;
			float xy = rot.X * ynorm;
			float xz = rot.X * znorm;
			float xw = rot.X * wnorm;
			float yy = rot.Y * ynorm;
			float yz = rot.Y * znorm;
			float yw = rot.Y * wnorm;
			float zz = rot.Z * znorm;
			float zw = rot.Z * wnorm;

			switch(i) {
				case 0:
					store.X  = 1 - 2 * (yy + zz);
					store.Y  =     2 * (xy + zw);
					store.Z  =     2 * (xz - yw);
					break;
				case 1:
					store.X  =     2 * (xy - zw);
					store.Y  = 1 - 2 * (xx + zz);
					store.Z  =     2 * (yz + xw);
					break;
				case 2:
					store.X  =     2 * (xz + yw);
					store.Y  =     2 * (yz - xw);
					store.Z  = 1 - 2 * (xx + yy);
					break;
				default:
					store = Vector3.Zero;
					break;
			}
		}

		/// <summary>
		/// Invert the specified quaternion.
		/// </summary>
		/// <param name="value">Source quaternion</param>
		/// <returns>Inverted quaternion</returns>
		public static Quaternion Invert(Quaternion value) {
			float lengthSquared = (value.X * value.X) + (value.Y * value.Y) + (value.Z * value.Z) + (value.W * value.W);
			float invSqrt = 1.0f / lengthSquared;

			Quaternion q;
			q.X = -value.X * invSqrt;
			q.Y = -value.Y * invSqrt;
			q.Z = -value.Z * invSqrt;
			q.W = -value.W * invSqrt;
			return q;
		}

		/// <summary>
		/// Invert the specified quaternion.
		/// </summary>
		/// <param name="value">Source quaternion</param>
		/// <param name="result">Existing quaternion to hold result</param>
		public static void Invert(ref Quaternion value, out Quaternion result) {
			float lengthSquared = (value.X * value.X) + (value.Y * value.Y) + (value.Z * value.Z) + (value.W * value.W);
			float invSqrt = 1.0f / lengthSquared;

			result.X = -value.X * invSqrt;
			result.Y = -value.Y * invSqrt;
			result.Z = -value.Z * invSqrt;
			result.W = -value.W * invSqrt;
		}

		/// <summary>
		/// Normalizes this quaternion.
		/// </summary>
		public void Normalize() {
			float lengthSquared = (this.X * this.X) + (this.Y * this.Y) + (this.Z * this.Z) + (this.W * this.W);
			float invSqrt = 1.0f / (float) System.Math.Sqrt((double) lengthSquared);
			this.X *= invSqrt;
			this.Y *= invSqrt;
			this.Z *= invSqrt;
			this.W *= invSqrt;
		}

		/// <summary>
		/// Normalizes the specified quaternion.
		/// </summary>
		/// <param name="value">Source quaternion</param>
		/// <returns>Resulting quaternion</returns>
		public static Quaternion Normalize(Quaternion value) {
			float lengthSquared = (value.X * value.X) + (value.Y * value.Y) + (value.Z * value.Z) + (value.W * value.W);
			float invSqrt = 1.0f / (float) System.Math.Sqrt((double) lengthSquared);

			Quaternion q;
			q.X = value.X * invSqrt;
			q.Y = value.Y * invSqrt;
			q.Z = value.Z * invSqrt;
			q.W = value.W * invSqrt;
			return q;
		}

		/// <summary>
		/// Normalizes the specified quaternion.
		/// </summary>
		/// <param name="value">Source quaternion</param>
		/// <param name="result">Existing quaternion to hold result</param>
		public static void Normalize(ref Quaternion value, out Quaternion result) {
			float lengthSquared = (value.X * value.X) + (value.Y * value.Y) + (value.Z * value.Z) + (value.W * value.W);
			float invSqrt = 1.0f / (float) System.Math.Sqrt((double) lengthSquared);

			result.X = value.X * invSqrt;
			result.Y = value.Y * invSqrt;
			result.Z = value.Z * invSqrt;
			result.W = value.W * invSqrt;
		}

		/// <summary>
		/// Lineraly interpolate between two quaternions.
		/// </summary>
		/// <param name="start">Starting quaternion</param>
		/// <param name="end">Ending quaternion</param>
		/// <param name="percent">Percent to interpolate between the two quaternions</param>
		/// <returns>Interpolated quaternion</returns>
		public static Quaternion Lerp(Quaternion start, Quaternion end, float percent) {
			float sx = start.X;
			float sy = start.Y;
			float sz = start.Z;
			float sw = start.W;

			float ex = end.X;
			float ey = end.Y;
			float ez = end.Z;
			float ew = end.W;

			float percent2 = 1.0f - percent;

			float num = (sx * ex) + (sy * ey) + (sz * ez) + (sw * ew);

			Quaternion q;

			if(num >= 0.0f) {
				q.X = (percent2 * sx) + (percent * ex);
				q.Y = (percent2 * sy) + (percent * ey);
				q.Z = (percent2 * sz) + (percent * ez);
				q.W = (percent2 * sw) + (percent * ew);
			} else {
				q.X = (percent2 * sx) - (percent * ex);
				q.Y = (percent2 * sy) - (percent * ey);
				q.Z = (percent2 * sz) - (percent * ez);
				q.W = (percent2 * sw) - (percent * ew);
			}

			float lengthSquared = (q.X * q.X) + (q.Y * q.Y) + (q.Z * q.Z) + (q.W * q.W);
			float invSqrt = 1.0f / (float) System.Math.Sqrt((double) lengthSquared);

			q.X *= invSqrt;
			q.Y *= invSqrt;
			q.Z *= invSqrt;
			q.W *= invSqrt;
			return q;
		}

		/// <summary>
		/// Lineraly interpolate between two quaternions.
		/// </summary>
		/// <param name="start">Starting quaternion</param>
		/// <param name="end">Ending quaternion</param>
		/// <param name="percent">Percent to interpolate between the two quaternions</param>
		/// <param name="result">Existing quaternion to hold result</param>
		/// <returns>Interpolated quaternion</returns>
		public static void Lerp(ref Quaternion start, ref Quaternion end, float percent, out Quaternion result) {

			float sx = start.X;
			float sy = start.Y;
			float sz = start.Z;
			float sw = start.W;

			float ex = end.X;
			float ey = end.Y;
			float ez = end.Z;
			float ew = end.W;

			float percent2 = 1.0f - percent;

			float num = (sx * ex) + (sy * ey) + (sz * ez) + (sw * ew);

			if(num >= 0.0f) {
				result.X = (percent2 * sx) + (percent * ex);
				result.Y = (percent2 * sy) + (percent * ey);
				result.Z = (percent2 * sz) + (percent * ez);
				result.W = (percent2 * sw) + (percent * ew);
			} else {
				result.X = (percent2 * sx) - (percent * ex);
				result.Y = (percent2 * sy) - (percent * ey);
				result.Z = (percent2 * sz) - (percent * ez);
				result.W = (percent2 * sw) - (percent * ew);
			}

			float lengthSquared = (result.X * result.X) + (result.Y * result.Y) + (result.Z * result.Z) + (result.W * result.W);
			float invSqrt = 1.0f / (float) System.Math.Sqrt((double) lengthSquared);

			result.X *= invSqrt;
			result.Y *= invSqrt;
			result.Z *= invSqrt;
			result.W *= invSqrt;
		}

		/// <summary>
		/// Interpolates between two quaternions using spherical linear interpolation.
		/// </summary>
		/// <param name="start">Starting quaternion</param>
		/// <param name="end">Ending quaternion</param>
		/// <param name="percent">Percent to interpolate between the two quaternions</param>
		/// <returns>Resulting interpolated quaternion</returns>
		public static Quaternion Slerp(Quaternion start, Quaternion end, float percent) {
			
			float sx = start.X;
			float sy = start.Y;
			float sz = start.Z;
			float sw = start.W;

			float ex = end.X;
			float ey = end.Y;
			float ez = end.Z;
			float ew = end.W;

			float amt1 = 0;
			float amt2 = 0;

			Quaternion q;

			float num = (sx * ex) + (sy * ey) + (sz * ez) + (sw * ew);
			bool flag = false;

			if(num < 0.0f) {
				flag = true;
				num = -num;
			}
			if(num > 0.999999f) {
				float t0 = 0;
				amt2 = 1.0f - percent;
				if(flag) {
					t0 = -percent;
				} else {
					t0 = percent;
				}
				amt1 = t0;
			} else {
				float t1 = 0;
				float t2 = (float) System.Math.Acos((double) num);
				float t3 = (float) (1.0d / System.Math.Sin((double) t2));
				amt2 = ((float) System.Math.Sin((1.0d - percent) * t2)) * t3;
				if(flag) {
					t1 = ((float) -System.Math.Sin(t2 * percent)) * t3;
				} else {
					t1 = ((float) System.Math.Sin(t2 * percent)) * t3;
				}
				amt1 = t1;
			}

			q.X = (ex * amt1) + (sx * amt2);
			q.Y = (ey * amt1) + (sy * amt2);
			q.Z = (ez * amt1) + (sz * amt2);
			q.W = (ew * amt1) + (sw * amt2);
			return q;
		}

		/// <summary>
		/// Interpolates between two quaternions using spherical linear interpolation.
		/// </summary>
		/// <param name="start">Starting quaternion</param>
		/// <param name="end">Ending quaternion</param>
		/// <param name="percent">Percent to interpolate between the two quaternions</param>
		/// <param name="result">Existing quaternion to hold result</param>
		public static void Slerp(ref Quaternion start, ref Quaternion end, float percent, out Quaternion result) {
			float sx = start.X;
			float sy = start.Y;
			float sz = start.Z;
			float sw = start.W;

			float ex = end.X;
			float ey = end.Y;
			float ez = end.Z;
			float ew = end.W;

			float amt1 = 0;
			float amt2 = 0;

			float num = (sx * ex) + (sy * ey) + (sz * ez) + (sw * ew);
			bool flag = false;

			if(num < 0.0f) {
				flag = true;
				num = -num;
			}
			if(num > 0.999999f) {
				float t0 = 0;
				amt2 = 1.0f - percent;
				if(flag) {
					t0 = -percent;
				} else {
					t0 = percent;
				}
				amt1 = t0;
			} else {
				float t1 = 0;
				float t2 = (float) System.Math.Acos((double) num);
				float t3 = (float) (1.0d / System.Math.Sin((double) t2));
				amt2 = ((float) System.Math.Sin((1.0d - percent) * t2)) * t3;
				if(flag) {
					t1 = ((float) -System.Math.Sin(t2 * percent)) * t3;
				} else {
					t1 = ((float) System.Math.Sin(t2 * percent)) * t3;
				}
				amt1 = t1;
			}

			result.X = (ex * amt1) + (sx * amt2);
			result.Y = (ey * amt1) + (sy * amt2);
			result.Z = (ez * amt1) + (sz * amt2);
			result.W = (ew * amt1) + (sw * amt2);
		}

		/// <summary>
		/// Creates a quaternion from an axis and an angle to rotate about that axis.
		/// </summary>
		/// <param name="angle">Angle to rotate, in radians</param>
		/// <param name="axis">Axis to rotate around</param>
		/// <returns>Resulting Quaternion</returns>
		public static Quaternion FromAngleAxis(float angle, Vector3 axis) {

			Quaternion q;

			float halfAngle = angle * 0.5f;
			float sin = (float) System.Math.Sin((double) halfAngle);
			float cos = (float) System.Math.Cos((double) halfAngle);

			q.X = axis.X * sin;
			q.Y = axis.Y * sin;
			q.Z = axis.Z * sin;
			q.W = cos;

			return q;
		}

		/// <summary>
		/// Creates a quaternion from an axis and an angle to rotate about that axis.
		/// </summary>
		/// <param name="angle">Angle to rotate, in radians</param>
		/// <param name="axis">Axis to rotate around</param>
		/// <param name="result">Existing quaternion to hold result</param>
		public static void FromAngleAxis(float angle, ref Vector3 axis, out Quaternion result) {
			float halfAngle = angle * 0.5f;
			float sin = (float) System.Math.Sin((double) halfAngle);
			float cos = (float) System.Math.Cos((double) halfAngle);

			result.X = axis.X * sin;
			result.Y = axis.Y * sin;
			result.Z = axis.Z * sin;
			result.W = cos;
		}

		/// <summary>
		/// Creates a quaternion from a matrix that contains a rotation.
		/// </summary>
		/// <param name="m">Source matrix</param>
		/// <returns>Resulting quaternion</returns>
		public static Quaternion FromRotationMatrix(Matrix m) {
			float m11 = m.M11;
			float m12 = m.M12;
			float m13 = m.M13;

			float m21 = m.M21;
			float m22 = m.M22;
			float m23 = m.M23;

			float m31 = m.M31;
			float m32 = m.M32;
			float m33 = m.M33;

			float trace = m11 + m22 + m33;
			Quaternion q = new Quaternion();
			if (trace > 0.0f)
			{
				float s1 = (float) System.Math.Sqrt((double) (trace + 1.0f));
				q.W = s1 * 0.5f;
				s1 = 0.5f / s1;
				q.X = (m23 - m32) * s1;
				q.Y = (m31 - m13) * s1;
				q.Z = (m12 - m21) * s1;
				return q;
			}
			if ((m11 >= m22) && (m11 >= m33))
			{
				float s2 = (float) System.Math.Sqrt((double) (((1.0f + m11) - m22) - m33));
				s2 = 0.5f / s2;
				q.X = 0.5f * s2;
				q.Y = (m12 + m21) * s2;
				q.Z = (m13 + m31) * s2;
				q.W = (m23 - m32) * s2;
				return q;
			}
			if (m22 > m33)
			{
				float s3 = (float) System.Math.Sqrt((double) (((1.0f + m22) - m11) - m33));
				s3 = 0.5f / s3;
				q.X = (m21 + m12) * s3;
				q.Y = 0.5f * s3;
				q.Z = (m32 + m23) * s3;
				q.W = (m31 - m13) * s3;
				return q;
			}
			float s4 = (float) System.Math.Sqrt((double) (((1.0f + m33) - m11) - m22));
			s4 = 0.5f / s4;
			q.X = (m31 + m13) * s4;
			q.Y = (m32 + m23) * s4;
			q.Z = 0.5f * s4;
			q.W = (m12 - m21) * s4;
			return q;
		}

		/// <summary>
		/// Creates a rotation quaternion where the object is facing the target along its z axis.
		/// If your object's "forward" facing is down -Z, then the object will correctly face the target.
		/// </summary>
		/// <param name="position">Position of object</param>
		/// <param name="target">Position of target</param>
		/// <param name="worldUp">World yAxis vector</param>
		/// <returns>Resulting rotation quaternion</returns>
		public static Quaternion LookAt(Vector3 position, Vector3 target, Vector3 worldUp) {
			Vector3 zAxis;
			Vector3 xAxis;
			Vector3 yAxis;

			Vector3.Subtract(ref position, ref target, out zAxis);
			zAxis.Normalize();

			Vector3 worldUpXdir;
			Vector3.Cross(ref worldUp, ref zAxis, out worldUpXdir);
			Vector3.Normalize(ref worldUpXdir, out xAxis);
			Vector3.Cross(ref zAxis, ref xAxis, out yAxis);

			Quaternion q;
			FromAxes(ref xAxis, ref yAxis, ref zAxis, out q);
			return q;
		}

		/// <summary>
		/// Creates a rotation quaternion where the object is facing the target along its z axis.
		/// If your object's "forward" facing is down -Z, then the object will correctly face the target.
		/// </summary>
		/// <param name="position">Position of object</param>
		/// <param name="target">Position of target</param>
		/// <param name="worldUp">World yAxis vector</param>
		/// <param name="result">Existing quaternion to hold result</param>
		public static void LookAt(ref Vector3 position, ref Vector3 target, ref Vector3 worldUp, out Quaternion result) {
			Vector3 zAxis;
			Vector3 xAxis;
			Vector3 yAxis;

			Vector3.Subtract(ref position, ref target, out zAxis);
			zAxis.Normalize();

			Vector3 worldUpXdir;
			Vector3.Cross(ref worldUp, ref zAxis, out worldUpXdir);
			Vector3.Normalize(ref worldUpXdir, out xAxis);
			Vector3.Cross(ref zAxis, ref xAxis, out yAxis);

			FromAxes(ref xAxis, ref yAxis, ref zAxis, out result);
		}

		/// <summary>
		/// Creates a quaternion from a matrix that contains a rotation.
		/// </summary>
		/// <param name="m">Source matrix</param>
		/// <param name="result">Existing quaternion to hold result</param>
		public static void FromRotationMatrix(ref Matrix m, out Quaternion result) {
			float m11 = m.M11;
			float m12 = m.M12;
			float m13 = m.M13;

			float m21 = m.M21;
			float m22 = m.M22;
			float m23 = m.M23;

			float m31 = m.M31;
			float m32 = m.M32;
			float m33 = m.M33;

			float trace = m11 + m22 + m33;
			if (trace > 0.0f) {
				float s1 = (float) System.Math.Sqrt((double) (trace + 1.0f));
				result.W = s1 * 0.5f;
				s1 = 0.5f / s1;
				result.X = (m23 - m32) * s1;
				result.Y = (m31 - m13) * s1;
				result.Z = (m12 - m21) * s1;
			} else if ((m11 >= m22) && (m11 >= m33)) {
				float s2 = (float) System.Math.Sqrt((double) (((1.0f + m11) - m22) - m33));
				s2 = 0.5f / s2;
				result.X = 0.5f * s2;
				result.Y = (m12 + m21) * s2;
				result.Z = (m13 + m31) * s2;
				result.W = (m23 - m32) * s2;
			} else if (m22 > m33) {
				float s3 = (float) System.Math.Sqrt((double) (((1.0f + m22) - m11) - m33));
				s3 = 0.5f / s3;
				result.X = (m21 + m12) * s3;
				result.Y = 0.5f * s3;
				result.Z = (m32 + m23) * s3;
				result.W = (m31 - m13) * s3;
			} else {
				float s4 = (float) System.Math.Sqrt((double) (((1.0f + m33) - m11) - m22));
				s4 = 0.5f / s4;
				result.X = (m31 + m13) * s4;
				result.Y = (m32 + m23) * s4;
				result.Z = 0.5f * s4;
				result.W = (m12 - m21) * s4;
			}
		}

		/// <summary>
		/// Creates a quaternion from euler angles yaw, pitch, roll.
		/// </summary>
		/// <param name="yaw">Rotation about y-axis, in radians</param>
		/// <param name="pitch">Rotation about x-axis, in radians</param>
		/// <param name="roll">Rotation about z-axis, in radians</param>
		/// <returns>Resulting quaternion</returns>
		public static Quaternion FromEulerAngles(float yaw, float pitch, float roll) {
			float angle = roll * 0.5f;
			float rollSin = (float) System.Math.Sin((double) angle);
			float rollCos = (float) System.Math.Cos((double) angle);

			angle = pitch * 0.5f;
			float pitchSin = (float) System.Math.Sin((double) angle);
			float pitchCos = (float) System.Math.Cos((double) angle);

			angle = yaw * 0.5f;
			float yawSin = (float) System.Math.Sin((double) angle);
			float yawCos = (float) System.Math.Cos((double) angle);

			float yawCosXpitchSin = yawCos * pitchSin;
			float yawSinXpitchCos = yawSin * pitchCos;
			float yawCosXpitchCos = yawCos * pitchCos;
			float yawSinXpitchSin = yawSin * pitchSin;

			Quaternion q;
			q.X = (yawCosXpitchSin * rollCos) + (yawSinXpitchCos * rollSin);
			q.Y = (yawSinXpitchCos * rollCos) - (yawCosXpitchSin * rollSin);
			q.Z = (yawCosXpitchCos * rollSin) - (yawSinXpitchSin * rollCos);
			q.W = (yawCosXpitchCos * rollCos) + (yawSinXpitchSin * rollSin);
			return q;
		}

		/// <summary>
		/// Creates a quaternion from euler angles yaw, pitch, and roll.
		/// </summary>
		/// <param name="yaw">Rotation about y-axis, in radians</param>
		/// <param name="pitch">Rotation about x-axis, in radians</param>
		/// <param name="roll">Rotation about z-axis, in radians</param>
		/// <param name="result">Existing quaternion to hold result</param>
		public static void FromEulerAngles(float yaw, float pitch, float roll, out Quaternion result) {
			float angle = roll * 0.5f;
			float rollSin = (float) System.Math.Sin((double) angle);
			float rollCos = (float) System.Math.Cos((double) angle);

			angle = pitch * 0.5f;
			float pitchSin = (float) System.Math.Sin((double) angle);
			float pitchCos = (float) System.Math.Cos((double) angle);

			angle = yaw * 0.5f;
			float yawSin = (float) System.Math.Sin((double) angle);
			float yawCos = (float) System.Math.Cos((double) angle);

			float yawCosXpitchSin = yawCos * pitchSin;
			float yawSinXpitchCos = yawSin * pitchCos;
			float yawCosXpitchCos = yawCos * pitchCos;
			float yawSinXpitchSin = yawSin * pitchSin;

			result.X = (yawCosXpitchSin * rollCos) + (yawSinXpitchCos * rollSin);
			result.Y = (yawSinXpitchCos * rollCos) - (yawCosXpitchSin * rollSin);
			result.Z = (yawCosXpitchCos * rollSin) - (yawSinXpitchSin * rollCos);
			result.W = (yawCosXpitchCos * rollCos) + (yawSinXpitchSin * rollSin);
		}

		/// <summary>
		/// Creates a quaternion that represents a rotation formed by three axes. It is assumed 
		/// the axes are orthogonal to one another and represent a proper xAxis-hand system.
		/// </summary>
		/// <param name="xAxis">X axis of the coordinate system (right)</param>
		/// <param name="yAxis">Y axis of the coordinate system (up)</param>
		/// <param name="zAxis">Z axis of the coordinate system (backward)</param>
		/// <returns>Resulting quaternion</returns>
		public static Quaternion FromAxes(Vector3 xAxis, Vector3 yAxis, Vector3 zAxis) {
			Matrix m = new Matrix(xAxis.X, xAxis.Y, xAxis.Z, 0, yAxis.X, yAxis.Y, yAxis.Z, 0, zAxis.X, zAxis.Y, zAxis.Z, 0, 0, 0, 0, 1);
			Quaternion q = new Quaternion();
			FromRotationMatrix(ref m, out q);
			return q;
		}

		/// <summary>
		/// Creates a quaternion that represents a rotation formed by three axes. It is assumed 
		/// the axes are orthogonal to one another and represent a proper xAxis-hand system.
		/// </summary>
		/// <param name="xAxis">X axis of the coordinate system</param>
		/// <param name="yAxis">Y axis of the coordinate system</param>
		/// <param name="zAxis">Z axis of the coordinate system</param>
		/// <param name="result">Existing quaternion that holds the result</param>
		public static void FromAxes(ref Vector3 xAxis, ref Vector3 yAxis, ref Vector3 zAxis, out Quaternion result) {
			Matrix m = new Matrix(xAxis.X, xAxis.Y, xAxis.Z, 0, yAxis.X, yAxis.Y, yAxis.Z, 0, zAxis.X, zAxis.Y, zAxis.Z, 0, 0, 0, 0, 1);
			FromRotationMatrix(ref m, out result);
		}

		/// <summary>
		/// Tests equality with another quaternion.
		/// </summary>
		/// <param name="value">Quaternion to compare</param>
		/// <returns>True if components are equal</returns>
		public bool Equals(Quaternion value) {
			return ((((this.X == value.X) && (this.Y == value.Y)) && (this.Z == value.Z)) && (this.W == value.W));
		}

		/// <summary>
		/// Tests equality between two quaternions.
		/// </summary>
		/// <param name="a">First quaternion</param>
		/// <param name="b">Second quaternion</param>
		/// <returns>True if components are equal</returns>
		public static bool operator==(Quaternion a, Quaternion b) {
			return ((((a.X == b.X) && (a.Y == b.Y)) && (a.Z == b.Z)) && (a.W == b.W));
		}

		/// <summary>
		/// Tests inequality between two quaternions.
		/// </summary>
		/// <param name="a">First quaternion</param>
		/// <param name="b">Second quaternion</param>
		/// <returns>True if components are not equal</returns>
		public static bool operator!=(Quaternion a, Quaternion b) {
			return !(a == b);
		}

		/// <summary>
		/// Adds the two quaternions together.
		/// </summary>
		/// <param name="a">First quaternion</param>
		/// <param name="b">Second quaternion</param>
		/// <returns>Added quaternion</returns>
		public static Quaternion operator+(Quaternion a, Quaternion b) {
			Quaternion q;
			q.X = a.X + b.X;
			q.Y = a.Y + b.Y;
			q.Z = a.Z + b.Z;
			q.W = a.W + b.W;
			return q;
		}

		/// <summary>
		/// Flips the signs of the components of the specified quaternion.
		/// </summary>
		/// <param name="value">Source quaternion</param>
		/// <returns>Negated quaternion</returns>
		public static Quaternion operator-(Quaternion value) {
			Quaternion q;
			q.X = -value.X;
			q.Y = -value.Y;
			q.Z = -value.Z;
			q.W = -value.W;
			return q; ;
		}

		/// <summary>
		/// Subtracts a quaternion from another.
		/// </summary>
		/// <param name="a">First quaternion</param>
		/// <param name="b">Second quaternion</param>
		/// <returns>Resulting quaternion</returns>
		public static Quaternion operator-(Quaternion a, Quaternion b) {
			Quaternion q;
			q.X = a.X - b.X;
			q.Y = a.Y - b.Y;
			q.Z = a.Z - b.Z;
			q.W = a.W - b.W;
			return q;
		}

		/// <summary>
		/// Multiplies two quaternions together.
		/// </summary>
		/// <param name="a">First quaternion</param>
		/// <param name="b">Second quaternion</param>
		/// <returns>Multiplied quaternion</returns>
		public static Quaternion operator*(Quaternion a, Quaternion b) {

			float x1 = a.X;
			float y1 = a.Y;
			float z1 = a.Z;
			float w1 = a.W;

			float x2 = b.X; 
			float y2 = b.Y; 
			float z2 = b.Z; 
			float w2 = b.W; 

			float num1 = (y1 * z2) - (z1 * y2);
			float num2 = (z1 * x2) - (x1 * z2);
			float num3 = (x1 * y2) - (y1 * x2);
			float num4 = ((x1 * x2) + (y1 * y2)) + (z1 * z2);

			Quaternion q;
			q.X = ((x1 * w2) + (x2 * w1)) + num1;
			q.Y = ((y1 * w2) + (y2 * w1)) + num2;
			q.Z = ((z1 * w2) + (z2 * w1)) + num3;
			q.W = (w1 * w2) - num4;

			return q;
		}

		/// <summary>
		/// Multiplies a quaternion by a scaling factor.
		/// </summary>
		/// <param name="value">Source quaternion</param>
		/// <param name="scale">Amount to multiply</param>
		/// <returns>Multiplied quaternion</returns>
		public static Quaternion operator*(Quaternion value, float scale) {
			Quaternion q;
			q.X = value.X * scale;
			q.Y = value.Y * scale;
			q.Z = value.Z * scale;
			q.W = value.W * scale;
			return q;
		}

		/// <summary>
		/// Multiplies a quaternion by a scaling factor.
		/// </summary>
		/// <param name="value">Source quaternion</param>
		/// <param name="scale">Amount to multiply</param>
		/// <returns>Multiplied quaternion</returns>
		public static Quaternion operator*(float scale, Quaternion value) {
			Quaternion q;
			q.X = value.X * scale;
			q.Y = value.Y * scale;
			q.Z = value.Z * scale;
			q.W = value.W * scale;
			return q;
		}

		/// <summary>
		/// Divides two quaternions.
		/// </summary>
		/// <param name="a">First quaternion</param>
		/// <param name="b">Second quaternion</param>
		/// <returns>Divided quaternion</returns>
		public static Quaternion operator/(Quaternion a, Quaternion b) {

			float x1 = a.X;
			float y1 = a.Y;
			float z1 = a.Z;
			float w1 = a.W;

			float x2 = b.X;
			float y2 = b.Y;
			float z2 = b.Z;
			float w2 = b.W;

			float num = (((x2 * x2) + (y2 * y2) + (z2 * z2) + (w2 * w2)));
			float invNum  = 1.0f / num;

			float num1 = -x2 * invNum;
			float num2 = -y2 * invNum;
			float num3 = -z2 * invNum;
			float num4 = -w2 * invNum;

			float num5 = (y1 * num3) - (z1 * num2);
			float num6 = (z1 * num1) - (x1 * num3);
			float num7 = (x1 * num2) - (y1 * num1);
			float num8 = ((x1 * num1) + (y1 * num2) + (z1 * num3));

			Quaternion q;
			q.X = ((x1 * num4) + (num1 * w1)) + num5;
			q.Y = ((y1 * num4) + (num2 * w1)) + num6;
			q.Z = ((z1 * num4) + (num3 * w1)) + num7;
			q.W = (w1 * num4) - num8;
			return q;
		}

		/// <summary>
		/// Tests equality between this quaternion and the supplied object.
		/// </summary>
		/// <param name="obj">Object to compare</param>
		/// <returns>True if the object is a quaternion and contents are equal</returns>
		public override bool Equals(object obj) {
			if(obj is Quaternion) {
				return Equals((Quaternion) obj);
			}
			return false;
		}

		/// <summary>
		/// Get the object's hash code.
		/// </summary>
		/// <returns>Hash code</returns>
		public override int GetHashCode() {
			return (this.X.GetHashCode() + this.Y.GetHashCode() + this.Z.GetHashCode() + this.W.GetHashCode());
		}

		/// <summary>
		/// Get a string representation for the object.
		/// </summary>
		/// <returns>String representation</returns>
		public override String ToString() {
			CultureInfo info = CultureInfo.CurrentCulture;
			return String.Format(info, "{{X:{0} Y:{1} Z:{2} W:{3}}}",
				new Object[] { this.X.ToString(info), this.Y.ToString(info), this.Z.ToString(info), this.W.ToString(info) });
		}
	}
}
